﻿using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;


namespace ORCA
{



    public class ObstacleSegment2D 
    {
        public Vector2 start;
        public Vector2 end;
    }

    public class ObstacleAgent2D 
    {
        public Vector2 position;
        public Vector2 velocity;
        public Vector2 desireVelocity;
        public float radius;
    }

    public struct Line
    {
        public Vector2 point;
        public Vector2 normal;
    }









    /// <summary>
    /// 这是实现ORCA碰撞算法相关的静态函数的
    /// </summary>
    public static class ORCA
    {
        /// <summary>
        /// 
        /// </summary>
        /// <param name="obstacleLines">物体周围的线段障碍</param>
        /// <param name="obstacleAgents">物体周围的圆形障碍</param>
        /// <param name="MyAgent">进行回避的物体</param>
        /// <param name="Acceleration">单位加速度（用来计算修改速度的可行值）</param>
        /// <param name="Deltatime">调用回避算法的时间间隔</param>
        /// <param name="avoidTime">考虑的回避时间</param>
        /// <returns></returns>
        /// 
        static float RVOepsilon = 0.001f;

        static float Determinant(Vector2 v1, Vector2 v2)
        {
            return v1.x * v2.y - v1.y * v2.x;
        }

        public static Vector2 MapV3ToV2(Vector3 v3)
        {
            return new Vector2(v3.x, v3.z);
        }

        public static Vector3 MapV2ToV3(Vector2 v2, float y)
        {
            return new Vector3(v2.x, y, v2.y);
        }


        public static Vector2 GetNewVelocity(ObstacleSegment2D[] obstacleLines, ObstacleAgent2D[] obstacleAgents, ObstacleAgent2D myAgent, float acceleration,float deltaTime,float avoidTime)
        {


            Line[] agentConstraint = GetObstacleAgentsConstaints(obstacleAgents, myAgent, avoidTime, deltaTime);
            Line[] linesConstraint = GetObstacleLinesConstraints(obstacleLines, myAgent, avoidTime, deltaTime);
            Line[] allConstraint = agentConstraint.Concat(linesConstraint).ToArray(); //LINQ连接数组

            float MaxDeltaVelocity = 100*acceleration * deltaTime;
            Vector2 newDeltaVelocity = new Vector2();

            //将约束变换到以当前速度为原点的坐标系，因为平移，法线不变
            for(int i=0;i< allConstraint.Length;i++)
            {
                allConstraint[i].point -= myAgent.velocity;
            }

            int lineFail = linearProgram2(allConstraint, MaxDeltaVelocity, myAgent.desireVelocity-myAgent.velocity, false, ref newDeltaVelocity);
            if (lineFail < allConstraint.Length)
            {
                linearProgram3(allConstraint, allConstraint.Length, lineFail, MaxDeltaVelocity, ref newDeltaVelocity);
            }
            return myAgent.velocity+newDeltaVelocity;
        }

        private static Line[] GetObstacleLinesConstraints(ObstacleSegment2D[] obstacleLines, ObstacleAgent2D MyAgent, float avoidTime,float deltaTime)
        {
            //TODO 单位与线段之间的碰撞回避
            return new Line[0];
        }

        private static Line[] GetObstacleAgentsConstaints(ObstacleAgent2D[] obstacleAgents, ObstacleAgent2D MyAgent, float avoidTime,float deltaTime)
        {
            Line[] lines = new Line[obstacleAgents.Length];
            for(int i=0;i< obstacleAgents.Length;i++)
            {
                Vector2 relativePosition = obstacleAgents[i].position - MyAgent.position;
                Vector2 relativeVelocity = MyAgent.velocity - obstacleAgents[i].velocity;
                float combineRadius = obstacleAgents[i].radius + MyAgent.radius;
                float combineRadiusSqr = combineRadius * combineRadius;
                float distanceSqr = (obstacleAgents[i].position - MyAgent.position).sqrMagnitude;
                float VOradius = combineRadius / avoidTime;
                Vector2 VOcenter = relativePosition / avoidTime;
                Vector2 voc2rv = relativeVelocity - VOcenter;
                Vector2 u;

                if (distanceSqr < combineRadiusSqr)
                {
                    //发生碰撞
                    //发生碰撞时VO已经包括了原点，回避就是使相对速度相离
                    //此时按照最近的VO(以deltaTime作为时间参数)建立约束
                    Vector2 closestVoCenter = relativePosition / deltaTime;
                    Vector2 cvoc2rv = relativeVelocity - closestVoCenter;
                    Vector2 cvoc2rv_nor = cvoc2rv.normalized;
                    lines[i].normal = new Vector2(cvoc2rv_nor.y, -cvoc2rv_nor.x);
                    u = (combineRadius / deltaTime) * cvoc2rv_nor - cvoc2rv;
                }
                else
                {
                    Vector2 voc2rv_nor = voc2rv.normalized;
                    //点在半圆的靠原点一边而且点在两条切点轴的靠原点一侧，则点到半圆表面的距离是到VO表面的距离的最短距离
                    if (Vector3.Dot(voc2rv, relativePosition) < 0 && Vector3.Project(voc2rv, voc2rv).sqrMagnitude > combineRadiusSqr)
                    {
                        //最近点在完整VO的半球上
                        lines[i].normal = new Vector2(voc2rv_nor.y, -voc2rv_nor.x);
                        u = (combineRadius /avoidTime ) * voc2rv_nor - voc2rv;
                    }
                    else
                    {
                        //最近点在完整VO的圆锥的两条边上
                        //定理：二维向量的位置关系满足右手螺旋定则（拇指离开纸面）时，以它们为列的矩阵的行列式值必为正
                        float legLength = Mathf.Sqrt(relativePosition.sqrMagnitude - combineRadiusSqr);
                        if (Determinant(relativePosition, voc2rv) > 0)
                        {
                            //离左边更近
                            lines[i].normal = new Vector2(relativePosition.x * legLength - relativePosition.y * combineRadius, relativePosition.x * combineRadius + relativePosition.y * legLength) / relativePosition.sqrMagnitude;
                        }
                        else
                        {
                            //离右边更近
                            lines[i].normal = -new Vector2(relativePosition.x * legLength + relativePosition.y * combineRadius, -relativePosition.x * combineRadius + relativePosition.y * legLength) / relativePosition.sqrMagnitude;
                        }
                        u = Vector2.Dot(relativeVelocity, lines[i].normal) * lines[i].normal - relativeVelocity;
                    }
                }
                lines[i].point = MyAgent.velocity + 0.5f * u;

            }



            return lines;
        }



        //线性规划程序用现成的
        private static bool linearProgram1(IList<Line> lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
        {
            float dotProduct = Vector2.Dot(lines[lineNo].point, lines[lineNo].normal);
            float discriminant = dotProduct * dotProduct + radius * radius - lines[lineNo].point.magnitude * lines[lineNo].point.magnitude;

            if (discriminant < 0.0f)
            {
                /* Max speed circle fully invalidates line lineNo. */
                return false;
            }

            float sqrtDiscriminant = Mathf.Sqrt(discriminant);
            float tLeft = -dotProduct - sqrtDiscriminant;
            float tRight = -dotProduct + sqrtDiscriminant;

            for (int i = 0; i < lineNo; ++i)
            {

                float denominator = Determinant(lines[lineNo].normal, lines[i].normal);
                float numerator = Determinant(lines[i].normal, lines[lineNo].point - lines[i].point);

                if (Mathf.Abs(denominator) <= RVOepsilon)
                {
                    /* Lines lineNo and i are (almost) parallel. */
                    if (numerator < 0.0f)
                    {
                        return false;
                    }

                    continue;
                }

                float t = numerator / denominator;

                if (denominator >= 0.0f)
                {
                    /* Line i bounds line lineNo on the right. */
                    tRight = Mathf.Min(tRight, t);
                }
                else
                {
                    /* Line i bounds line lineNo on the left. */
                    tLeft = Mathf.Max(tLeft, t);
                }

                if (tLeft > tRight)
                {
                    return false;
                }
            }

            if (directionOpt)
            {
                /* Optimize direction. */
                if (Vector2.Dot(optVelocity, lines[lineNo].normal) > 0.0f)
                {
                    /* Take right extreme. */
                    result = lines[lineNo].point + tRight * lines[lineNo].normal;
                }
                else
                {
                    /* Take left extreme. */
                    result = lines[lineNo].point + tLeft * lines[lineNo].normal;
                }
            }
            else
            {
                /* Optimize closest point. */
                float t = Vector2.Dot(lines[lineNo].normal, (optVelocity - lines[lineNo].point));

                if (t < tLeft)
                {
                    result = lines[lineNo].point + tLeft * lines[lineNo].normal;
                }
                else if (t > tRight)
                {
                    result = lines[lineNo].point + tRight * lines[lineNo].normal;
                }
                else
                {
                    result = lines[lineNo].point + t * lines[lineNo].normal;
                }
            }

            return true;
        }
        private static int linearProgram2(IList<Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
        {
            if (directionOpt)
            {
                /*
                 * Optimize direction. Note that the optimization velocity is of
                 * unit length in this case.
                 */
                result = optVelocity * radius;
            }
            else if (optVelocity.magnitude * optVelocity.magnitude > radius * radius)
            {
                /* Optimize closest point and outside circle. */
                result = optVelocity.normalized * radius;
            }
            else
            {
                /* Optimize closest point and inside circle. */
                result = optVelocity;
            }

            for (int i = 0; i < lines.Count; ++i)
            {
                if (Determinant(lines[i].normal, lines[i].point - result) > 0.0f)
                {
                    /* Result does not satisfy constraint i. Compute new optimal result. */
                    Vector2 tempResult = result;
                    if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
                    {
                        result = tempResult;

                        return i;
                    }
                }
            }

            return lines.Count;
        }
        private static void linearProgram3(IList<Line> lines, int numObstLines, int beginLine, float radius, ref Vector2 result)
        {
            float distance = 0.0f;

            for (int i = beginLine; i < lines.Count; ++i)
            {
                if (Determinant(lines[i].normal, lines[i].point - result) > distance)
                {
                    /* Result does not satisfy constraint of line i. */
                    IList<Line> projLines = new List<Line>();
                    for (int ii = 0; ii < numObstLines; ++ii)
                    {
                        projLines.Add(lines[ii]);
                    }

                    for (int j = numObstLines; j < i; ++j)
                    {
                        Line line;

                        float determinant = Determinant(lines[i].normal, lines[j].normal);

                        if (Mathf.Abs(determinant) <= RVOepsilon)
                        {
                            /* Line i and line j are parallel. */
                            if (Vector2.Dot(lines[i].normal, lines[j].normal) > 0.0f)
                            {
                                /* Line i and line j point in the same direction. */
                                continue;
                            }
                            else
                            {
                                /* Line i and line j point in opposite direction. */
                                line.point = 0.5f * (lines[i].point + lines[j].point);
                            }
                        }
                        else
                        {
                            line.point = lines[i].point + (Determinant(lines[j].normal, lines[i].point - lines[j].point) / determinant) * lines[i].normal;
                        }

                        line.normal = (lines[j].normal - lines[i].normal).normalized;
                        projLines.Add(line);
                    }

                    Vector2 tempResult = result;
                    if (linearProgram2(projLines, radius, new Vector2(-lines[i].normal.y, lines[i].normal.x), true, ref result) < projLines.Count)
                    {
                        /*
                         * This should in principle not happen. The result is by
                         * definition already in the feasible region of this
                         * linear program. If it fails, it is due to small
                         * floating point error, and the current result is kept.
                         */
                        result = tempResult;
                    }

                    distance = Determinant(lines[i].normal, lines[i].point - result);
                }
            }
        }

        #region 未修改的逻辑


        //private static bool linearProgram1(IList<Line> lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
        //{
        //    float dotProduct = Vector2.Dot(lines[lineNo].point, lines[lineNo].normal);
        //    float discriminant = dotProduct * dotProduct + radius * radius - lines[lineNo].point.magnitude * lines[lineNo].point.magnitude;

        //    if (discriminant < 0.0f)
        //    {
        //        /* Max speed circle fully invalidates line lineNo. */
        //        return false;
        //    }

        //    float sqrtDiscriminant = Mathf.Sqrt(discriminant);
        //    float tLeft = -dotProduct - sqrtDiscriminant;
        //    float tRight = -dotProduct + sqrtDiscriminant;

        //    for (int i = 0; i < lineNo; ++i)
        //    {

        //        float denominator = Determinant(lines[lineNo].normal, lines[i].normal);
        //        float numerator = Determinant(lines[i].normal, lines[lineNo].point - lines[i].point);

        //        if (Mathf.Abs(denominator) <= RVOepsilon)
        //        {
        //            /* Lines lineNo and i are (almost) parallel. */
        //            if (numerator < 0.0f)
        //            {
        //                return false;
        //            }

        //            continue;
        //        }

        //        float t = numerator / denominator;

        //        if (denominator >= 0.0f)
        //        {
        //            /* Line i bounds line lineNo on the right. */
        //            tRight = Mathf.Min(tRight, t);
        //        }
        //        else
        //        {
        //            /* Line i bounds line lineNo on the left. */
        //            tLeft = Mathf.Max(tLeft, t);
        //        }

        //        if (tLeft > tRight)
        //        {
        //            return false;
        //        }
        //    }

        //    if (directionOpt)
        //    {
        //        /* Optimize direction. */
        //        if (Vector2.Dot(optVelocity, lines[lineNo].normal) > 0.0f)
        //        {
        //            /* Take right extreme. */
        //            result = lines[lineNo].point + tRight * lines[lineNo].normal;
        //        }
        //        else
        //        {
        //            /* Take left extreme. */
        //            result = lines[lineNo].point + tLeft * lines[lineNo].normal;
        //        }
        //    }
        //    else
        //    {
        //        /* Optimize closest point. */
        //        float t = Vector2.Dot(lines[lineNo].normal, (optVelocity - lines[lineNo].point));

        //        if (t < tLeft)
        //        {
        //            result = lines[lineNo].point + tLeft * lines[lineNo].normal;
        //        }
        //        else if (t > tRight)
        //        {
        //            result = lines[lineNo].point + tRight * lines[lineNo].normal;
        //        }
        //        else
        //        {
        //            result = lines[lineNo].point + t * lines[lineNo].normal;
        //        }
        //    }

        //    return true;
        //}
        //private static int linearProgram2(IList<Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
        //{
        //    if (directionOpt)
        //    {
        //        /*
        //         * Optimize direction. Note that the optimization velocity is of
        //         * unit length in this case.
        //         */
        //        result = optVelocity * radius;
        //    }
        //    else if (optVelocity.magnitude * optVelocity.magnitude > radius * radius)
        //    {
        //        /* Optimize closest point and outside circle. */
        //        result = optVelocity.normalized * radius;
        //    }
        //    else
        //    {
        //        /* Optimize closest point and inside circle. */
        //        result = optVelocity;
        //    }

        //    for (int i = 0; i < lines.Count; ++i)
        //    {
        //        if (Determinant(lines[i].normal, lines[i].point - result) > 0.0f)
        //        {
        //            /* Result does not satisfy constraint i. Compute new optimal result. */
        //            Vector2 tempResult = result;
        //            if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
        //            {
        //                result = tempResult;

        //                return i;
        //            }
        //        }
        //    }

        //    return lines.Count;
        //}
        //private static void linearProgram3(IList<Line> lines, int numObstLines, int beginLine, float radius, ref Vector2 result)
        //{
        //    float distance = 0.0f;

        //    for (int i = beginLine; i < lines.Count; ++i)
        //    {
        //        if (Determinant(lines[i].normal, lines[i].point - result) > distance)
        //        {
        //            /* Result does not satisfy constraint of line i. */
        //            IList<Line> projLines = new List<Line>();
        //            for (int ii = 0; ii < numObstLines; ++ii)
        //            {
        //                projLines.Add(lines[ii]);
        //            }

        //            for (int j = numObstLines; j < i; ++j)
        //            {
        //                Line line;

        //                float determinant = Determinant(lines[i].normal, lines[j].normal);

        //                if (Mathf.Abs(determinant) <= RVOepsilon)
        //                {
        //                    /* Line i and line j are parallel. */
        //                    if (Vector2.Dot(lines[i].normal, lines[j].normal) > 0.0f)
        //                    {
        //                        /* Line i and line j point in the same direction. */
        //                        continue;
        //                    }
        //                    else
        //                    {
        //                        /* Line i and line j point in opposite direction. */
        //                        line.point = 0.5f * (lines[i].point + lines[j].point);
        //                    }
        //                }
        //                else
        //                {
        //                    line.point = lines[i].point + (Determinant(lines[j].normal, lines[i].point - lines[j].point) / determinant) * lines[i].normal;
        //                }

        //                line.normal = (lines[j].normal - lines[i].normal).normalized;
        //                projLines.Add(line);
        //            }

        //            Vector2 tempResult = result;
        //            if (linearProgram2(projLines, radius, new Vector2(-lines[i].normal.y, lines[i].normal.x), true, ref result) < projLines.Count)
        //            {
        //                /*
        //                 * This should in principle not happen. The result is by
        //                 * definition already in the feasible region of this
        //                 * linear program. If it fails, it is due to small
        //                 * floating point error, and the current result is kept.
        //                 */
        //                result = tempResult;
        //            }

        //            distance = Determinant(lines[i].normal, lines[i].point - result);
        //        }
        //    }
        //}
        #endregion
    }
}
